package src.bojan.planning;

import static src.bojan.ConfigurationParams.*;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import sim.field.continuous.Continuous2D;
import sim.util.Double2D;
import src.bojan.TerrainMap;
import bojan.robots.motion.DistanceEvaluator;
import ec.util.MersenneTwisterFast;

public class Planner {
	// TODO Usar el de simulation
	MersenneTwisterFast mtw = new MersenneTwisterFast(
			System.currentTimeMillis());
	private Continuous2D grid;
	private TerrainMap map;
	double radius;
	double threshold;
	private int maxGoals;

	public Planner(Continuous2D grid, TerrainMap map, double radius,
			double threshold, int maxGoals) {
		super();
		this.grid = grid;
		this.map = map;
		this.radius = radius;
		this.threshold = threshold;
		this.maxGoals = maxGoals;
	}

	public Double2D nextGoal() {
		Double2D nextGoal = null;
		// TODO decrease threshold if not able to find.
		while (nextGoal == null) {
			Double2D possible = random();
			if (map.visitedInZone(possible, radius) < threshold
					|| mtw.nextDouble() < 0.05) { // 3% chance of going to an unpromising area
				nextGoal = possible;
			}
		}
		return nextGoal;
	}

	public Double2D random() {
		return new Double2D(range( grid.width), range( grid.height));
	}

	public double range(double width) {
		return (mtw.nextDouble() * (width * 1.05)) - width *  0.05;
	}

	public Set<Double2D> proposeGoals(int maxNewGoals) {
		Set<Double2D> a = new HashSet();
		for (int i = 0; i < maxNewGoals; i++) {
			a.add(nextGoal());
		}
		return a;
	}

	public List<Double2D> initialTour(Double2D position) { 
		return tour(position, proposeGoals(maxGoals));
	}
	
	public List<Double2D> tour(Double2D position, Collection<Double2D> goals) {
		ArrayList<Double2D> tour = new ArrayList<Double2D>();
		while (!goals.isEmpty()) {
			Double2D closest = new DistanceEvaluator().closest(goals, position);
			tour.add(closest);			
			goals.remove(closest);
			position = closest;
		}
		return tour;
	}

	public double estimatedCost(Double2D position, Double2D goal) {
		return position.distance(goal) * 1.1; // TODO 1.5 as a param
	}

	public List<Double2D> insertGoals(Double2D position, List<Double2D> goals,
			Collection<Double2D> newGoals) {
		Collection<Double2D> a = new ArrayList<Double2D>();
		a.addAll(goals);
		a.addAll(newGoals);
		return tour(position, a);
	}

	public List<Double2D> updateTour(Double2D position, List<Double2D> goals) {
		if (goals.size() < MAX_NEW_GOALS) {
			return insertGoals(position, goals,
					proposeGoals(MAX_NEW_GOALS - goals.size()));
			
		} 
		return tour(position, goals);
	}

	public double value(Double2D goal) {
		return map.unknownInZone(goal, 2.0) * CELL_VALUE_WEIGHT;
	}

}
